//
//  cmd_packet.c
//  CmdPacket
//
//  Created by hua on 2021/12/12.
//

#include "cmd_packet.h"

void pkg_dump(uint8_t *data, int size) {
    int i=0;
    for (; i<size; i++) {
        if (i != 0 && i % 16 == 0) {
            printf("\n");
        }
        printf("%02X ", data[i]);
    }
    printf("\n");
}

static uint8_t crc8(uint8_t *data, int size)
{
    uint8_t i;
    uint8_t crc = 0;        // Initial value
    while(size--)
    {
        crc ^= *data++;        // crc ^= *data; data++;
        for ( i = 0; i < 8; i++ )
        {
            if ( crc & 0x80 )
                crc = (crc << 1) ^ 0x07;
            else
                crc <<= 1;
        }
    }
    return crc;
}


// 对数据进行转义，转义的数据存在out缓存 out可以设置为data的两倍大小
int pkg_encode(uint8_t *out, CmdPacket *pkg) {
    uint8_t *u8p = (uint8_t *)pkg;
    int i = 0, j = 0;
    // 自动
    pkg->begin = CMD_PACKET_BEGIN;
    pkg->data[pkg->len] = crc8(u8p + 1, pkg->len + 1); // LEN也要要做CS
    pkg->data[pkg->len + 1] = CMD_PACKET_ENG;
    
    out[j++] = u8p[i++]; // 第一个字节
    
//    pkg_dump(u8p, sizeof(CmdPacket) + pkg->len + 2);
    
    int stop = 2 + pkg->len + 1; // LEN本身也要转义 CS也要转义
    
//    printf("len=%d,stop=%d\n", pkg->len, stop);

    for (; i<stop; i++) {
        uint8_t c = u8p[i];
//        printf("i=%d,c=%02X\n", i, c);
        switch (c) {
            case CMD_PACKET_BEGIN:
                out[j++] = CMD_PACKET_GENG;
                out[j++] = CMD_PACKET_BEGIN_REPLACE;
                break;
            case CMD_PACKET_GENG:
                out[j++] = CMD_PACKET_GENG;
                out[j++] = CMD_PACKET_GENG_REPLACE;
                break;
            case CMD_PACKET_ENG:
                out[j++] = CMD_PACKET_GENG;
                out[j++] = CMD_PACKET_ENG_REPLACE;
                break;
            default:
                out[j++] = u8p[i];
                break;
        }
    }
    out[j++] = u8p[i];
    return j;
}


// 反转义 共用了数据空间
CmdPacket *pkg_decode(uint8_t *out, int size) {
    // 第一字节和最后一个字节不用转义
    uint8_t i = 1, j = 1; //
    uint8_t stop =  size - 1;
//    printf("size=%d\n", size);
    uint8_t prev_is_geng = 0;
    // 01 03 01 03 01 00
    for (; i<stop; i++) {
        uint8_t c = out[i];
//        printf("i=%d,%d,%02X\n", i, j, c);
        if (prev_is_geng) {
            switch (c) {
                case CMD_PACKET_BEGIN_REPLACE:
                    out[j++] = CMD_PACKET_BEGIN;
//                    printf("begin\n");
                    break;
                case CMD_PACKET_GENG_REPLACE:
                    out[j++] = CMD_PACKET_GENG;
//                    printf("gengr\n");
                    break;
                case CMD_PACKET_ENG_REPLACE:
                    out[j++] = CMD_PACKET_ENG;
//                    printf("end\n");
                    break;
                default:
                    // 转义错误
//                    printf("decode err!");
                    return NULL;
            }
            prev_is_geng = 0;
        } else {
            if (c == CMD_PACKET_GENG) {
//                printf("geng\n");
                prev_is_geng = 1;
            } else {
                out[j++] = out[i];
            }
        }
    }
//    FF 03 02 02 02 02 C8 00
    out[j++] = out[i];
    CmdPacket *pkg = (CmdPacket *)out;
    uint8_t cs = crc8(out + 1, j - 3);
    if (pkg->data[pkg->len] != cs) {
        printf("cs:%d,%02X!=%02X\n", j, pkg->data[pkg->len], cs);
        return NULL;
    }
    return pkg;
}



int pkg_send(CmdPacket * pkg, uint8_t *out) {
    int size = pkg_encode(out, pkg);
    printf("send:\n");
    pkg_dump(out, size);
    return size;
}

CmdPacket *pkg_recv(uint8_t *input, int size) {
    CmdPacket * pkg = pkg_decode(input, size);
    if (pkg) {
        printf("recv:\n");
        pkg_dump(input, sizeof(CmdPacket) + pkg->len + 2);
    } else {
        printf("recv err!\n");
    }
    return pkg;
}
